﻿using Advantech.Motion;
using Advantech.MotionComponent;
using DLLPC.Utilities;
using OfficeOpenXml.FormulaParsing.Excel.Functions.RefAndLookup;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Security.Policy;
using System.Text;
using System.Threading;
using System.Threading.Tasks;

namespace VisionCore
{
    public class AxisHelper_SMC
    {
        public clsAxisMotionI AxisMotionI = new clsAxisMotionI();
        public clsAxisMotionO AxisMotionO = new clsAxisMotionO();
        public StepData[] StepDataNos { get; set; } = new StepData[10];

        private int dblActPos = 0;
        public int CurrentActPos
        {
            get
            {
                GetActPosition(ref dblActPos);
                return dblActPos;
            }
        }

        private int dblActForce = 0;
        public int CurrentActForce
        {
            get
            {
                GetActForce(ref dblActForce);
                return dblActForce;
            }
        }

        public bool MoveInp { get; set; } = false;
        public bool PositioningError { get; set; } = false;

        private SMCInfo axis_SMC = new SMCInfo();
        private MotionHelper motion;
        private ushort slaveIP = 0;
        bool isRun = true;
        Thread thread_AxisRun;
        public AxisHelper_SMC(MotionHelper motion, SMCInfo axis)
        {
            this.motion = motion;
            axis_SMC = axis;

            thread_AxisRun = new Thread(ThreadFunc_Run);
            thread_AxisRun.IsBackground = true;
            thread_AxisRun.Start();

            StepDataNos[0] = new StepData(0, motion, axis_SMC.SlaveIP);

            //for (int i = 0; i < StepDataNos.Length; i++)
            //{
            //    StepDataNos[i] = new StepData(i, motion, axis_SMC.SlaveIP);
            //}
        }

        int runStatus = 0;
        int runMode = 0;

        SystemTimeCounter scTime = new SystemTimeCounter();
        public void AxisRun(int mode, int runCode)
        {
            runMode = mode;
            if (runMode == 2)
            {
                byte[] code = new byte[1];
                code[0] = (byte)runCode;
                AxisMotionO.IN0 = MemoryAccess.GetBit(code, 0, 0);
                AxisMotionO.IN1 = MemoryAccess.GetBit(code, 0, 1);
                AxisMotionO.IN2 = MemoryAccess.GetBit(code, 0, 2);
                AxisMotionO.IN3 = MemoryAccess.GetBit(code, 0, 3);
                AxisMotionO.IN4 = MemoryAccess.GetBit(code, 0, 4);
                AxisMotionO.IN5 = MemoryAccess.GetBit(code, 0, 5);
            }
            runStatus = 1;
        }

        public void AxisRMRun(int runCode)
        {
            runMode = runCode;
            if (runMode == 2)
            {

                motion.WriteSDOData(0, 0x13, (ushort)(0x200B), 1, 1);
            }
        
        }


        public void AxisStop()
        {
            runStatus = 0;
            AxisMotionO.SETUP = 0;
            AxisMotionO.DRIVE = 0;
        }

        object objLock = new object();
        public void ResetALM()
        {
            PositioningError = false;
            lock (objLock)
            {
                Task.Run(() =>
                {
                    AxisMotionO.RST = 1;
                    Thread.Sleep(500);

                    AxisMotionO.RST = 0;
                });
            }
        }

        void ThreadFunc_Run()
        {
            while (isRun)
            {
                switch (runStatus)
                {
                    case 0:
                        break;
                    case 1:
                        {
                            MoveInp = false;
                            if (runMode == 1)
                            {
                                AxisMotionO.SETUP = 1;
                            }
                            else if (runMode == 2)
                            {
                                Thread.Sleep(20);
                                AxisMotionO.DRIVE = 1;
                            }
                            runStatus = 2;
                            scTime.Start();
                        }
                        break;
                    case 2:
                        {
                            if (AxisMotionI.BUSY == 1)
                            {
                                AxisMotionO.SETUP = 0;
                                AxisMotionO.DRIVE = 0;
                                runStatus = 3;
                                scTime.Start();
                            }
                            else
                            {
                                if (scTime.Duration > 1000)
                                {
                                    AxisMotionO.SETUP = 0;
                                    AxisMotionO.DRIVE = 0;
                                    runStatus = 0;
                                    LogMsg("等待Busy超时？");
                                }
                            }
                        }
                        break;
                    case 3:
                        {
                            if (AxisMotionI.INP == 1)
                            {
                                MoveInp = true;
                                runStatus = 0;
                            }
                            else
                            {
                                if (scTime.Duration > 3000)
                                {
                                    runStatus = 0;
                                    PositioningError = true;
                                    LogMsg("等待定位完成超时？");
                                }
                            }
                        }
                        break;
                }

                SystemTimeCounter.timeBeginPeriod(1);
                Thread.Sleep(1);
                SystemTimeCounter.timeEndPeriod(1);
            }
        }

        void ResetCode()
        {
            AxisMotionO.IN0 = 0;
            AxisMotionO.IN1 = 0;
            AxisMotionO.IN2 = 0;
            AxisMotionO.IN3 = 0;
            AxisMotionO.IN4 = 0;
            AxisMotionO.IN5 = 0;
        }

        public void GetMotionIO()
        {
            uint MotionIO = 0;
            bool ret = false;
            ret = motion.GetDIValue(0, axis_SMC.StartPort_I, ref MotionIO);

            #region 
            AxisMotionI.OUT0 = GetOnOff(MotionIO, 0);
            AxisMotionI.OUT1 = GetOnOff(MotionIO, 1);
            AxisMotionI.OUT2 = GetOnOff(MotionIO, 2);
            AxisMotionI.OUT3 = GetOnOff(MotionIO, 3);
            AxisMotionI.OUT4 = GetOnOff(MotionIO, 4);
            AxisMotionI.OUT5 = GetOnOff(MotionIO, 5);
            AxisMotionI.BUSY = GetOnOff(MotionIO, 8);
            AxisMotionI.SVON = GetOnOff(MotionIO, 9);
            AxisMotionI.SETON = GetOnOff(MotionIO, 10);
            AxisMotionI.INP = GetOnOff(MotionIO, 11);
            AxisMotionI.AREA = GetOnOff(MotionIO, 12);
            AxisMotionI.WAREA = GetOnOff(MotionIO, 13);
            AxisMotionI.EMG = GetOnOff(MotionIO, 14);
            AxisMotionI.ALM = GetOnOff(MotionIO, 15);

            AxisMotionI.RDY = GetOnOff(MotionIO, 20);
            #endregion
        }

        private void GetActPosition(ref int actPos)
        {
            uint MotionIO = 0;
            if (motion.GetDIValue(0, (ushort)(axis_SMC.StartPort_I + 4), ref MotionIO))
            {
                actPos = (int)MotionIO;
            }
        }

        private void GetActForce(ref int actForce)
        {
            uint MotionIO = 0;
            if (motion.GetDIValue(0, (ushort)(axis_SMC.StartPort_I + 10), ref MotionIO))
            {
                actForce = (int)(MotionIO & 0xffff);
            }
        }

        public void SetMotionIO()
        {
            ushort MotionIO = 0;
            bool ret = false;

            SetBitShort(ref MotionIO, AxisMotionO.IN0, 0);
            SetBitShort(ref MotionIO, AxisMotionO.IN1, 1);
            SetBitShort(ref MotionIO, AxisMotionO.IN2, 2);
            SetBitShort(ref MotionIO, AxisMotionO.IN3, 3);
            SetBitShort(ref MotionIO, AxisMotionO.IN4, 4);
            SetBitShort(ref MotionIO, AxisMotionO.IN5, 5);
            SetBitShort(ref MotionIO, AxisMotionO.SVON, 9);
            SetBitShort(ref MotionIO, AxisMotionO.DRIVE, 10);
            SetBitShort(ref MotionIO, AxisMotionO.RST, 11);
            SetBitShort(ref MotionIO, AxisMotionO.SETUP, 12);
            SetBitShort(ref MotionIO, AxisMotionO.JOG_R, 13);
            SetBitShort(ref MotionIO, AxisMotionO.JOG_F, 14);

            motion.SetDOValue(0, axis_SMC.StartPort_O, MotionIO);
        }

        public void SetDOValue(ushort startPort, ushort doStatus)
        {
            uint Ret, Result;
            ushort num = 4;
            byte[] DOStatusBuf = new byte[num];
            byte[] DOStatus = new byte[num];
            bool DOUpdate = false;
            DOStatus[0] = (byte)(doStatus & 0xff);
            DOStatus[1] = (byte)(doStatus >> 8 & 0xff);
            if (motion.Devices[0].Handle != IntPtr.Zero)
            {
                Ret = Motion.mAcm_DaqDoGetBytes(motion.Devices[0].Handle, startPort, num, DOStatusBuf);
                if (Ret == (uint)ErrorCode.SUCCESS)
                {
                    for (int i = 0; i < num; i++)
                    {
                        if (DOStatus[i] != DOStatusBuf[i])
                        {
                            DOUpdate = true;
                        }
                    }

                    if (DOUpdate)
                    {
                        Thread.Sleep(20);
                        Result = Motion.mAcm_DaqDoSetBytes(motion.Devices[0].Handle, startPort, num, DOStatus);
                    }
                }
            }
        }

        private uint[] BitOnValue = new uint[32] { 1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80, 0x100, 0x200, 0x400, 0x800, 0x1000, 0x2000, 0x4000, 0x8000, 0x10000, 0x20000, 0x40000, 0x80000, 0x100000, 0x200000, 0x400000, 0x800000, 0x1000000, 0x2000000, 0x4000000, 0x8000000, 0x10000000, 0x20000000, 0x40000000, 0x80000000 };

        private static ushort[] BitOffVaule = new ushort[] { 0xFFFF - 1, 0xFFFF - 2, 0xFFFF - 4, 0xFFFF - 8, 0xFFFF - 0x10, 0xFFFF - 0x20, 0xFFFF - 0x40, 0xFFFF - 0x80, 0xFFFF - 0x100, 0xFFFF - 0x200, 0xFFFF - 0x400, 0xFFFF - 0x800, 0xFFFF - 0x1000, 0xFFFF - 0x2000, 0xFFFF - 0x4000, 0xFFFF - 0x8000 };

        private int GetOnOff(uint I, int BitSeq)
        {
            return (I & BitOnValue[BitSeq]) > 0 ? 1 : 0;
        }

        private void SetBitShort(ref ushort dataBuf, int intValue, int bitIdx)
        {
            if (intValue > 0)
            {
                dataBuf = (ushort)(dataBuf | BitOnValue[bitIdx]);
            }
            else
            {
                dataBuf = (ushort)(dataBuf & BitOffVaule[bitIdx]);
            }
        }

        public class StepData
        {
            private int stepNo = 0;
            private MotionHelper motion1;
            private ushort slaveIP;
            public StepData(int stepNo, MotionHelper motion, ushort slaveIP)
            {
                motion1 = motion;
                this.stepNo = stepNo;
                this.slaveIP = slaveIP;
            }
            private short speed = 0;
            public short Speed
            {
                get
                {
                    return speed;
                }
                set
                {
                    short speedBuf = value;

                    if (speed != speedBuf)
                    {
                        motion1.WriteSDOData(0, slaveIP, (ushort)(0x3000 + stepNo), 2, speedBuf);

                        motion1.ReadSDOData(0, slaveIP, (ushort)(0x3000 + stepNo), 2, ref speed);
                    }
                }
            }

            private int tarPos = 0;
            public int TarPos
            {
                get
                {
                    return tarPos;
                }
                set
                {
                    int tarBuf = value;

                    if (tarPos != tarBuf)
                    {
                        motion1.WriteSDOData(0, slaveIP, (ushort)(0x3000 + stepNo), 3, tarBuf);

                        motion1.ReadSDOData(0, slaveIP, (ushort)(0x3000 + stepNo), 3, ref tarPos);
                    }
                }
            }

            private short pushingForce = 0;
            public short PushingForce
            {
                get
                {
                    return pushingForce;
                }
                set
                {
                    short pushBuf = value;

                    if (pushingForce != pushBuf)
                    {
                        motion1.WriteSDOData(0, slaveIP, (ushort)(0x3000 + stepNo), 6, pushBuf);

                        motion1.ReadSDOData(0, slaveIP, (ushort)(0x3000 + stepNo), 6, ref pushingForce);
                    }
                }
            }

            private short triggerLV = 0;
            public short TriggerLV
            {
                get
                {
                    return triggerLV;
                }
                set
                {
                    short triggerBuf = value;

                    if (triggerLV != triggerBuf)
                    {
                        motion1.WriteSDOData(0, slaveIP, (ushort)(0x3000 + stepNo), 7, triggerBuf);

                        motion1.ReadSDOData(0, slaveIP, (ushort)(0x3000 + stepNo), 7, ref triggerLV);
                    }
                }
            }

            private short pushingSpeed = 0;
            public short PushingSpeed
            {
                get
                {
                    return pushingSpeed;
                }
                set
                {
                    short pushingSpeedBuf = value;

                    if (pushingSpeed != pushingSpeedBuf)
                    {
                        motion1.WriteSDOData(0, slaveIP, (ushort)(0x3000 + stepNo), 8, pushingSpeedBuf);

                        motion1.ReadSDOData(0, slaveIP, (ushort)(0x3000 + stepNo), 8, ref pushingSpeed);
                    }
                }
            }

            private short movingForce = 0;
            public short MovingForce
            {
                get
                {
                    return movingForce;
                }
                set
                {
                    short movingForceBuf = value;

                    if (movingForce != movingForceBuf)
                    {
                        motion1.WriteSDOData(0, slaveIP, (ushort)(0x3000 + stepNo), 9, movingForceBuf);

                        motion1.ReadSDOData(0, slaveIP, (ushort)(0x3000 + stepNo), 9, ref movingForce);
                    }
                }
            }

            private int inPos = 0;
            public int InPos
            {
                get
                {
                    return inPos;
                }
                set
                {
                    int inPosBuf = value;

                    if (inPos != inPosBuf)
                    {
                        motion1.WriteSDOData(0, slaveIP, (ushort)(0x3000 + stepNo), 12, inPosBuf);

                        motion1.ReadSDOData(0, slaveIP, (ushort)(0x3000 + stepNo), 12, ref inPos);
                    }
                }
            }
        }

        private void LogMsg(string Msg)
        {
            string strMsg = string.Format("TR Clamp {0} ", axis_SMC.SlaveIP) + Msg;
            LogHelper.Write(LogHelper.Module.Motion_Axis, strMsg);
        }
    }

    public class SMCInfo
    {
        public ushort StartPort_I { get; set; } = 0;
        public ushort NumPort_I { get; set; } = 0;
        public ushort StartPort_O { get; set; } = 1;
        public ushort NumPort_O { get; set; } = 1;
        public ushort SlaveIP { get; set; } = 1;
    }
}
